package physics;

import java.util.ArrayList;
import java.awt.Color;
import physics.Vector;
import physics.Vector.Type;

public abstract class physicsobject implements Forcer {
	/**
	 * 
	 */
	private static final long serialVersionUID = 1L;
	private double mass;
	private Vector momentum;
	private Vector newmomentum;
	private Location loc;
	private double charge;
	private Color col;
	private final double gravConstant = 6.667 * Math.pow(10, -11);
	
	public physicsobject(double mass, double charge, Vector momentum,
			Location loc) {
		this.charge = charge;
		this.mass = mass;
		this.momentum = momentum;
		newmomentum = momentum;
		this.loc = loc;
		col = Color.BLACK;
	}

	public Vector getNewmomentum() {
		return newmomentum;
	}

	public Color getColor() {
		return col;
	}

	public void setColor(Color c) {
		col = c;
	}

	public abstract boolean isColliding(physicsobject po);

	public double getMass() {
		return mass;
	}

	public Vector getMomentum() {
		return momentum;
	}

	public Vector getVelocity() {
		return momentum.divide(mass).toType(Vector.Type.VELOCITY);
	}

	public Location getLoc() {
		return loc;
	}

	public void setLoc(Location l) {
		loc = l;
	}

	public void setMass(double mass) {
		this.mass = mass;
	}

	public void setMomentum(Vector momentum) {
		this.momentum = momentum;
		newmomentum = momentum;
	}

	public void setCharge(double num) {
		charge = num;

	}

	public void applyForce(Vector f) {

		newmomentum = newmomentum.add(f.multiply(PhysicsEngine.TIMESTEP));

		// not all objects can have torques applied to them like FiziksBall
		if (isTorquable()) {
			doTorque(f);
		}
	}

	public void updateMomentum() {
		momentum = newmomentum;
	}
	
	public void updateLocation() {
		updateMomentum();
		loc = loc.addVector(momentum.multiply(PhysicsEngine.TIMESTEP / mass));
	}

	public boolean isTorquable() {
		return false;
	}

	public void doTorque(Vector force) {

	}

	public Vector[] getForces(physicsobject po) {
		// Vector[] ret = {getNormalForce(po), new
		// Vector(getLoc(),Vector.Type.FORCE_NORMAL,0,0)};
		if(PhysicsEngine.getPhysicsEngine().isStableCode()){
			Vector[] ret = { getElectricForce(po), getGravitation(po) };
			return ret;
		}
		else{
			Vector[] ret = { getNormalForce3(po),getElectricForce(po), getGravitation(po) };
			return ret;
		}
		
	}

	public Vector getNormalForce(physicsobject po) {
		if (po.isColliding(this)) {
			double m2 = po.getMass();
			double m1 = mass;
			double Vx1 = momentum.divide(mass).getXMag();
			double Vx2 = po.getMomentum().divide(po.getMass()).getXMag();
			double Vy1 = momentum.divide(mass).getYMag();
			double Vy2 = po.getMomentum().divide(po.getMass()).getYMag();
			double x1 = loc.getX();
			double y1 = loc.getY();
			double x2 = po.getLoc().getX();
			double y2 = po.getLoc().getY();
			double mratio = 0;
			if (m1 <= m2) {
				mratio = m1 / m2;
			} else {
				mratio = m2 / m1;
			}
			double xdiff = x2 - x1;
			double ydiff = y2 - y1;
			double Vxdiff = Vx2 - Vx1;
			double Vydiff = Vy2 - Vy1;

			if (!(xdiff * Vxdiff + ydiff * Vydiff >= 0)) {
				int sign;
				if (Math.abs(xdiff) < ydiff) {
					if (xdiff < 0) {
						sign = -1;
					} else {
						sign = 1;
					}
					xdiff = ydiff * sign;
				}
				double a = ydiff / xdiff;
				double dvx2 = -2 * (Vxdiff + a * Vydiff)
						/ ((1 + a * a) * (1 + mratio));
				double Vx2p = Vx2 + dvx2;
				double Vy2p = Vy2 + a * dvx2;
				double Vx1p = Vx1 - mratio * dvx2;
				double Vy1p = Vy1 - a * mratio * dvx2;

				double Fx2 = m2 * (Vx2p - Vx2) / PhysicsEngine.TIMESTEP;
				double Fy2 = m2 * (Vy2p - Vy2) / PhysicsEngine.TIMESTEP;
				double Fx1 = m1 * (Vx1p - Vx1) / PhysicsEngine.TIMESTEP;
				double Fy1 = m1 * (Vy1p - Vy1) / PhysicsEngine.TIMESTEP;
				Vector force1 = new Vector(loc, Vector.Type.FORCE_NORMAL, Fx1,
						Fy1);

				Vector force2 = new Vector(loc, Vector.Type.FORCE_NORMAL, Fx2,
						Fy2);
				this.applyForce(force1);
				po.applyForce(force2);
				return force2;
			}
		}
		return new Vector(loc, Vector.Type.FORCE_NORMAL, 0, 0);
	}

	public Vector getNormalForce2(physicsobject po) {
		double y1 = loc.getY();
		double y2 = po.getLoc().getY();
		double x1 = loc.getX();
		double x2 = po.getLoc().getX();
		// TODO Inspect whether the angle assignments are correct
		if (po.isColliding(this)) {
			System.out.println("Collided");
			double theta = Math.atan(-(po.getLoc().getX() - loc.getX())
					/ (po.getLoc().getY() - loc.getY()));

			if (y1 >= y2) {
				if (y1 == y2) {
					System.out.println("Error");
				}
				if (x1 <= x2) {// checked
					double theta2 = momentum.getAngle() - theta;
					double somag = momentum.getNetMag() * Math.sin(theta2);
					Vector so = new Vector(loc, Vector.Type.FORCE_NORMAL,
							somag, theta - Math.PI / 2, true);
					double theta3 = po.getMomentum().getAngle() - theta;
					double osmag = po.getNewmomentum().getNetMag()
							* Math.cos(theta3);
					Vector os = new Vector(loc, Vector.Type.FORCE_NORMAL,
							osmag, theta + Math.PI, true);
					return ((so.add(os)).add(po.getMomentum().multiply(-1)))
							.divide(PhysicsEngine.TIMESTEP);
				} else {// 
					double theta2 = momentum.getAngle() - theta;
					double theta3 = po.getMomentum().getAngle() - theta;
					double somag = momentum.getNetMag() * Math.sin(theta2);
					Vector so = new Vector(loc, Vector.Type.FORCE_NORMAL,
							somag, theta - Math.PI / 2, true);
					double osmag = po.getNewmomentum().getNetMag()
							* Math.cos(theta3);
					Vector os = new Vector(loc, Vector.Type.FORCE_NORMAL,
							osmag, theta, true);
					return ((so.add(os)).add(po.getMomentum().multiply(-1)))
							.divide(PhysicsEngine.TIMESTEP);

				}
			} else {// checked
				if (x1 <= x2) {
					double theta2 = momentum.getAngle() - theta;
					double theta3 = po.getMomentum().getAngle() - theta;
					double somag = momentum.getNetMag() * Math.sin(theta2);
					Vector so = new Vector(loc, Vector.Type.MOMENTUM, somag,
							theta + Math.PI / 2, true);
					double osmag = po.getNewmomentum().getNetMag()
							* Math.cos(theta3);
					Vector os = new Vector(loc, Vector.Type.MOMENTUM, osmag,
							theta + Math.PI, true);
					return ((so.add(os)).add(po.getMomentum().multiply(-1)))
							.divide(PhysicsEngine.TIMESTEP);

				} else {
					double theta2 = momentum.getAngle() - theta;
					double theta3 = po.getMomentum().getAngle() - theta;
					double somag = momentum.getNetMag() * Math.sin(theta2);
					Vector so = new Vector(loc, Vector.Type.MOMENTUM, somag,
							theta + Math.PI / 2, true);
					double osmag = po.getNewmomentum().getNetMag()
							* Math.cos(theta3);
					Vector os = new Vector(loc, Vector.Type.MOMENTUM, osmag,
							theta, true);
					return ((so.add(os)).add(po.getMomentum().multiply(-1)))
							.divide(PhysicsEngine.TIMESTEP);

				}
			}
		}
		return new Vector(loc, Vector.Type.FORCE_NORMAL, 0, 0);
	}

	// components thing
	public Vector getNormalForce3(physicsobject po) {
		if (!po.isColliding(this)) {
			return new Vector().toType(Type.FORCE_NORMAL);
		}
		Location one = getLoc();
		Location two = po.getLoc();
		double m1 = getMass();
		double m2 = po.getMass();
		// !

		double xdiff = two.getX() - one.getX();
		double ydiff = two.getY() - one.getY();

		double Vxdiff = po.getVelocity().getXMag() - getVelocity().getXMag();
		double Vydiff = po.getVelocity().getYMag() - getVelocity().getYMag();

		if (xdiff * Vxdiff + ydiff * Vydiff >= 0) {
			return new Vector().toType(Type.FORCE_NORMAL);
		}

		Vector un = new Vector(new Location(), null, two.getX() - one.getX(),
				two.getY() - one.getY()).unit();
		Vector ut = new Vector(new Location(), null, -un.getYMag(), un
				.getXMag());
		double v1nm = un.dot(getVelocity());
		double v1tm = ut.dot(getVelocity());
		double v2nm = un.dot(po.getVelocity());
		double v2tm = ut.dot(po.getVelocity());

		double v1nf = (v1nm * (m1 - m2) + 2 * m2 * v2nm) / (m1 + m2);
		double v2nf = (v2nm * (m2 - m1) + 2 * m1 * v1nm) / (m1 + m2);

		Vector v1f = un.multiply(v1nf).add(ut.multiply(v1tm));
		Vector v2f = un.multiply(v2nf).add(ut.multiply(v2tm));

		return v2f.multiply(po.getMass()).subtract(po.getNewmomentum()).divide(
				PhysicsEngine.TIMESTEP)
				.toType(Type.FORCE_NORMAL);
	}

	public double getCharge() {
		return charge;
	}

	public Vector getElectricForce(physicsobject objectCompare) {
		if (getCharge() == 0 || objectCompare.getCharge() == 0) {
			return new Vector(new Location(0, 0), Vector.Type.FORCE_ELECTRIC,
					0, 0);
		}
		double constantK = 9.0 * Math.pow(10.0, 9);
		Location loc2 = objectCompare.getLoc();
		Location loc1 = getLoc();
		double dist = loc1.distanceTo(loc2);
		double angle = loc1.angleTo(loc2);
		double charge2 = objectCompare.getCharge();
		double charge1 = getCharge();
		double totalMag = (constantK * charge1 * charge2) / (dist * dist);
		// System.out.println(totalMag);
		double xMag = Math.cos(angle) * totalMag;
		double yMag = Math.sin(angle) * totalMag;
		return new Vector(loc1, Vector.Type.FORCE_ELECTRIC, xMag, yMag);
	}

	public Vector getGravitation(physicsobject objectCompare) {
		double mass2 = objectCompare.getMass();
		Location mass2Loc = objectCompare.getLoc();
		double radius = loc.distanceTo(mass2Loc);
		double xMag = loc.getX() - objectCompare.getLoc().getX();
		double yMag = loc.getY() - objectCompare.getLoc().getY();
		Vector gravDirection = new Vector(objectCompare.getLoc(), null, xMag,
				yMag);
		gravDirection = gravDirection.unit();
		gravDirection = gravDirection.multiply(
				(gravConstant * mass * mass2) / Math.pow(radius, 2)).toType(
				Vector.Type.FORCE_GRAVITY);
		return gravDirection;
	}
}
